
/*****************************************************************************\
 *                Test for the collision pipeline
\*****************************************************************************/

/*! @file
 *
 *  @brief
 *    Move a number of objects around inside a cube.
 *
 *  This is an example program showing how to use the collision detection API.
 *  (This is similar to the original movem from Y.)
 *  The user can rotate / translate / zoom the cage.
 *  Only the center of objects will be checked against the "cage".
 *
 *  Type './movem -h' for an explanation of the command line options and keys.
 *
 *  @author Weiyu Yi 
 *
 *  @bugs
 *
 *  @flags
 *
 */

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <algorithm>

#if defined(__APPLE__)
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

#include <Collision.h>
#include <ColBoxtree.h>
#include <ColDopTree.h>
#include <ColTopology.h>
#include <ColUtils.h>
#include <ColUtilsOpenGL.h>

#ifdef _WIN32
#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif
#endif

using col::Quat;
using col::ColGeometry;
using col::Primitive;
using col::Cube;
using col::Icosahedron;
using col::Trackball;

using col::Point3;
using col::Vector3;
using col::Vector4;
using col::Matrix4;
using col::Boxtree;
using col::DopTree;

using col::makeCube;
using col::makeSphere;
using col::makePlane;
using col::drawGeometry;

class CollisionSyncFun; //Needed to synch during concurrent collision detection

/***************************************************************************\
 *                        Global variables                                 *
\***************************************************************************/

namespace
{

typedef enum
{
	PERSPECTIVE,
	ORTHOGRAPHIC
} ProjectionE;

typedef enum
{
	MODELING,
	VIEWING
} ControlModeE;

// here declaration of the global variables.
bool with_window = true;
bool animation_started = false;

bool moved = true;
vector<const std::vector<col::Point3> *> fixed_points;
vector<const std::vector<col::Point3> *> moving_points;

// render options
GLuint w, h;
ControlModeE ctrol = VIEWING;
ProjectionE proj = PERSPECTIVE;
GLenum fill_mode = GL_LINE;

Trackball tball;
GLdouble view_distance = 25.0;
Matrix4 viewing_matrix;

// the collision detection pipeline object
col::CollisionPipeline* pipeline;

// constants
const unsigned int 	MaxNumObjs = 500;

// Col. variables
col::AlgoE algo = col::ALGO_DOPTREE;
Matrix4 moving_matrix;
ColGeometry cage;

// options
unsigned int	nmovingobjs = 10;
unsigned int	complexity = 10;
double			cage_radius = 10;
bool			concurrent_coldet = false;
col::LevelOfDetectionE	level_of_det = col::LEVEL_EXACT;
bool            all_polygons = false;
bool            with_grid = false;
unsigned int	gridsize = 10;
unsigned int	NLoops = 1;
volatile bool   exitPipeline = false;

// multthreading support
boost::thread* pt; 
boost::mutex callback_mutex;
boost::barrier sync_barrier(2);

enum GeometryTypeE
{
	ALL_TYPES,
	OBJ_PLANES,
	OBJ_SPHERES,
	OBJ_TORUS,
	NUM_GEOM_TYPES
};
GeometryTypeE		geom_type = ALL_TYPES;

// state
unsigned int		ncollision = 0;
unsigned int		cur_frame_counter = 0;

// pseudo-random number generator (instead of drand48)
col::FibRand *      prng;

}


/***************************************************************************\
 * MovingObj
\***************************************************************************/

struct MovingObj
{
	// ctor, dtor,
	MovingObj();

	// methods
	void setRandomPos( void );
	void setRandomVel( void );
	void move( void );
	void makeCollidable( void );
	void setHit( const Vector3 &colnormal );
	void newVelocity( void );
	void draw() const;

	// state
	col::ColID			m_objID;
	bool				m_coll;
	Matrix4				m_mat;
	col::ColGeometry	m_geo;
	double				m_color[3];
	Vector3				m_transl_vel;	// translational velocity
	Matrix4				m_rot_vel;		// rotational velocity, derived from ...
	Vector3				m_rot_axis;
	double				m_rot_angle;
	Matrix4				m_oldmatrix;	// position of obj as of last frame
	unsigned int		m_hit;			// = true if hit during current frame
	Vector3				m_collision_normal;	// will be used to reflect velocity
	bool				m_initial_pos;	// initial position of obj
	bool				m_isFree;		// set if obj didn't have coll. last frame
};


MovingObj mobj[MaxNumObjs];


MovingObj::MovingObj()
:
	m_mat(),
	m_geo(),
	m_color(),
	m_transl_vel(),
	m_rot_vel(),
	m_rot_axis(),
	m_rot_angle(),
	m_oldmatrix(),
	m_hit(0),
	m_coll(false),
	m_collision_normal(),
	m_initial_pos(true),
	m_isFree(true)
{}

/** OpenGL wrapper function to draw the geometry of the moving obj
 */
void MovingObj::draw() const
{
	// drawing
	glPushMatrix();
	GLdouble mat[16];
	for ( unsigned int i=0; i<4; i++ )
	for ( unsigned int j=0; j<4; j++ )
		mat[ i*4+j ] = m_mat[i][j];
	glMultMatrixd( mat );

	col::drawGeometry( &m_geo, fill_mode, m_color );
	glPopMatrix();
}


/// register this object with the coll. det. module
void MovingObj::makeCollidable( void )
{
    m_objID = pipeline->makeCollidable( static_cast<void *>( &m_geo ), NULL );
}



/** move obj a little further. If outside cage, calc
 *  "collision normal" and call setHit().
 */
void MovingObj::move( void )
{
	m_oldmatrix = m_mat;
	Vector3 v_transl( m_mat[3][0], m_mat[3][1], m_mat[3][2] );
	v_transl += m_transl_vel;
	
	// m_mat.setCol( 3, Vector4(v_transl, 1.0) );
	m_mat.setTranslation( v_transl );
	m_mat = m_mat * m_rot_vel;

	Vector3 colnormal(0);
	for ( unsigned int i = 0; i < 3; i ++ )
	{
		if ( m_mat[3][i] > cage_radius )
			colnormal[i] = -1;
		else
		if ( m_mat[3][i] < -cage_radius )
			colnormal[i] =  1;
	}

	if ( col::NearZero < length( colnormal ) )
	{
		colnormal = normalize( colnormal );
		setHit( colnormal );
		// this hit must be handled, so we pretend the obj was free last time
		m_isFree = true;
	}

	pipeline->moveObject( m_objID, static_cast<void *>(&m_mat), NULL );
}


/** Save collision normal
 * @pre
 *   @a colnormal must have unit length.
 */
void MovingObj::setHit( const Vector3 &colnormal )
{
	m_hit = cur_frame_counter;			// does this work also with concurrent coll.det.?
	m_collision_normal = colnormal;
}



/** Reflect velocity at collision normal, if hit during current cycle, and
 *  copy old position into the transformation.
 */
void MovingObj::newVelocity( void )
{
	if ( m_hit != cur_frame_counter )
	{
		// don't need new velocity
		m_isFree = true;
		return;
	}

	if ( ! m_isFree )
		// yet another coll., so continue on path in order to avoid getting stuck
		return;

	m_isFree = false;

	// invert velocities
	m_transl_vel -= m_collision_normal *
		( dot( m_transl_vel, m_collision_normal ) * 2 );

	m_rot_angle = -m_rot_angle;

	m_rot_vel = Matrix4::rotation( m_rot_angle, m_rot_axis );
}


void MovingObj::setRandomPos( void )
{
	Vector4 randpnt( (prng->frand()-0.5) * cage_radius,
						(prng->frand()-0.5) * cage_radius,
						(prng->frand()-0.5) * cage_radius, 1 );
	m_mat.setCol( 3, randpnt );
}


void MovingObj::setRandomVel( void )
{
	m_transl_vel = Vector3( prng->frand()-0.5, prng->frand()-0.5, prng->frand()-0.5 );
	m_transl_vel *= cage_radius/20;
	m_rot_axis = Vector3( prng->frand()-0.5, prng->frand()-0.5, prng->frand()-0.5 );
	m_rot_axis = normalize( m_rot_axis );
	m_rot_angle = prng->frand() * 0.2;
	m_rot_vel = Matrix4::rotation( m_rot_angle, m_rot_axis );
}

/***************************************************************************\
 * Create Scene
\***************************************************************************/

void createScene()
{
	// create cage
	makeCube( &cage, cage_radius, true );

	// create moving objs
	double rad = 0.7;
	unsigned int type = 0;
	for ( unsigned int i=0; i<nmovingobjs; i++ )
	{
		type = type % 3;

		switch (type)
		{
			case 0:
			{
				// geo
				makeCube( &( mobj[i].m_geo ), rad );
				// matrix
				mobj[i].m_mat = Matrix4::identity();
				mobj[i].setRandomPos();
				mobj[i].setRandomVel();
				// color
				mobj[i].m_color[0] = prng->frand();
				mobj[i].m_color[1] = prng->frand();
				mobj[i].m_color[2] = prng->frand();

				break;
			}
			case 1:
			{
				// geo
				makeSphere( &( mobj[i].m_geo ), 1, rad );
				// matrix
				mobj[i].m_mat = Matrix4::identity();
				mobj[i].setRandomPos();
				mobj[i].setRandomVel();
				// color
				mobj[i].m_color[0] = prng->frand();
				mobj[i].m_color[1] = prng->frand();
				mobj[i].m_color[2] = prng->frand();

				break;
			}
			case 2:
			{
				// geo
				makePlane( &( mobj[i].m_geo ), 2*rad, 2*rad, 5, 5 );
				(*mobj[i].m_geo.editPointsPtr())[0][2] += 0.3;
				(*mobj[i].m_geo.editPointsPtr())[mobj[i].m_geo.getPointsPtr()->size()-1][2] -= 0.3;
				mobj[i].m_geo.updateBBox();
				// matrix
				mobj[i].m_mat = Matrix4::identity();
				mobj[i].setRandomPos();
				mobj[i].setRandomVel();
				// color
				mobj[i].m_color[0] = prng->frand();
				mobj[i].m_color[1] = prng->frand();
				mobj[i].m_color[2] = prng->frand();

				break;
			}
			default:
			{
				fprintf( stderr, "the programm shouldn't arrive here\n" );
			}
		}

		// 
		type ++;
	}
}

/***************************************************************************\
 * MovemCallback
\***************************************************************************/

class MovemCallback : public col::Callback
{
public:
	MovemCallback( MovingObj &obj1, MovingObj &obj2,
				   col::LevelOfDetectionE level );
	virtual ~MovemCallback();
	virtual void operator () (const col::Data *data) throw ();
	static void addCallback( MovingObj &obj1, MovingObj &obj2,
							 col::LevelOfDetectionE level );
protected:
	MovingObj & mobj1;
	MovingObj & mobj2;
};


MovemCallback::MovemCallback( MovingObj &inobj1, MovingObj &inobj2,
							  col::LevelOfDetectionE inlevel )
:	col::Callback(inobj1.m_objID, inobj2.m_objID, false, all_polygons, inlevel),
	mobj1(inobj1), mobj2(inobj2)
{
}


MovemCallback::~MovemCallback() {}


void MovemCallback::operator () (const col::Data *) throw ()
{
	if ( mobj1.m_objID != obj1ID || mobj2.m_objID != obj2ID  )
	{
		fputs("MovemCallback: mobj.node != obj !\n",stderr);
		return;
	}

	printf("Callback: %d <-> %d\n", mobj1.m_objID, mobj2.m_objID);
	
	callback_mutex.lock();

	// calc "collision normal"
	Vector3 pos1( mobj1.m_mat[3][0], mobj1.m_mat[3][1], mobj1.m_mat[3][2] );
	Vector3 pos2( mobj2.m_mat[3][0], mobj2.m_mat[3][1], mobj2.m_mat[3][2] );
	Vector3 collnormal( pos1 - pos2 );
	collnormal = normalize( collnormal );

	mobj1.setHit( collnormal );
	mobj2.setHit( collnormal );

	callback_mutex.unlock();

	ncollision ++ ;
}


void MovemCallback::addCallback( MovingObj &obj1, MovingObj &obj2,
								 col::LevelOfDetectionE level )
{
    pipeline->addCallback( new MovemCallback( obj1, obj2, level ) );
}


/***************************************************************************\
 * SyncFun
\***************************************************************************/

class CollisionSyncFun : public col::SyncFun
{
public:
	CollisionSyncFun( );
	virtual ~CollisionSyncFun();
	virtual bool operator () () throw ();
};

CollisionSyncFun::CollisionSyncFun( )
{
}

CollisionSyncFun::~CollisionSyncFun()
{
}

//Here you can implement synchronization between the collision pipeline
//and your own (main) thread. In this example it isn't much.
//You MUST implement the synchronization (i.e., exchange of changelists) somewhere!
//and this sync function is a good place to do this.
bool CollisionSyncFun::operator () ( ) throw ()
{
	if ( exitPipeline )
	{
		fprintf(stdout, "Collision thread: received exit signal.\n");
		return false;
	}

    if( cur_frame_counter > 0 )
	{
		// wait for the end of the rendering of the last frame of the scenegraph
		sync_barrier.wait();

		// effect the scenegraph with the result of the coll::check()
		// synchronizing data between 2 threads
		//
		// calc "collision normal"
		// ----------

		sync_barrier.wait();
		// proceed check-loop thread, at the same time the render thread too
	
		// debug...
		printf( "'\t\t\t\tcheck loop\n" );
	}

	cur_frame_counter ++ ;
	return true;
}


/***************************************************************************\
 *                       Check and  process coll                           *
\***************************************************************************/

void move_and_check( void )
{
	
	cur_frame_counter ++;

	pipeline->check();

	// add velocity delta on each pos
	for ( unsigned int i = 0; i < nmovingobjs; i ++ )
		mobj[i].move();

	for ( unsigned int i = 0; i < nmovingobjs; i ++ )
		mobj[i].newVelocity();
}

void movemExit( void )
{
	printf("num collisions = %u\n", ncollision );

    if( concurrent_coldet )
    {
        exitPipeline = true;
		//pt->join();
    }

	delete pipeline;
}

/***************************************************************************\
 *                       glut render process                               *
\***************************************************************************/

void init(void) 
{
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
}

void reshape( int width, int height )
{
	w = width;
	h = height;
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	tball.setViewport( w, h );
}

void animate( void )
{
	if ( animation_started == false )
		return;

	if ( concurrent_coldet == false )
		move_and_check();
	else
	{
		// concurrent
		sync_barrier.wait();

		// sychronzing...

		sync_barrier.wait();
		// then update and render the scenegraph

		// debug...
		printf( "update scene...\n" );
		
		callback_mutex.lock();
		for ( unsigned int i = 0; i < nmovingobjs; i ++ )
			mobj[i].move();
		for ( unsigned int i = 0; i < nmovingobjs; i ++ )
			mobj[i].newVelocity();

		callback_mutex.unlock();
	}

	glutPostRedisplay();
}

void display(void)
{
	// trackball 
	if ( ctrol == VIEWING )
	{
		if ( tball.getTrackMode() == col::TRACK_ROTATE )
		{
			REAL angle;
			REAL axis[3];
			tball.getRotation( axis, angle );
			Matrix4 m = Matrix4::rotation( angle, col::Vector3( axis[0], axis[1], axis[2] ) );
			viewing_matrix = m * viewing_matrix;
		}
		else
		if ( tball.getTrackMode() == col::TRACK_ZOOM )
		{
			REAL zoom;
			tball.getZoom( zoom );
			Matrix4 m = Matrix4::scale( col::Vector3( zoom+1 ) );
			viewing_matrix = m * viewing_matrix;
		}
	}

	// all geometries are ready to render
	// --------------------------------------------------------
	// display begins
	
	// Prjection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity ();

	GLdouble range=20.0;
	switch( proj )
	{
		case ORTHOGRAPHIC:
			// orthographic projection
			if (w <= h)
				glOrtho(-1*range, range, -1*range*(GLdouble)h/(GLdouble)w,
						range*(GLdouble)h/(GLdouble)w, 0.1, 100.0);
			else
				glOrtho(-1*range*(GLdouble)w/(GLdouble)h,
						range*(GLdouble)w/(GLdouble)h, -1*range, range, 0.1, 100.0);
			break;
		case PERSPECTIVE:
			// perspective projection
			gluPerspective(60, (GLdouble)w/(GLdouble)h, 0.1, 100.0);
			break;
		default:
			fprintf( stderr, "No such projection mode!\n" );
			break;
	}

	glMatrixMode( GL_MODELVIEW );
	glLoadIdentity();
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	// View Transformation
	glTranslated( 0.0, 0.0, -view_distance );

	GLdouble view_mat[16];
	for ( unsigned int i=0; i<4; i++ )
	for ( unsigned int j=0; j<4; j++ )
		view_mat[ i*4+j ] = viewing_matrix[i][j];
	glMultMatrixd( view_mat );

	// Modelling Transformation
	
	// draw cage
	double cage_color[3] = { 0.9, 0.9, 0.9 };
	drawGeometry( &cage, GL_LINE, cage_color );

	// draw moving objs
	for ( unsigned int i=0; i<nmovingobjs; i++ )
	{
		mobj[i].draw();
	}

	//
	glutSwapBuffers();
	
}

void mouseClick( int button, int state, int x, int y )
{
	tball.mouseClick( button, state, x, y );
}

void mouseMoving( int x, int y )
{
	moved = true;
	tball.mouseMoving( x, y );
	glutPostRedisplay();
}


void keyboard(unsigned char key, int x, int y)
{
	static int phase = -1;

	switch (key) {
		case 27:
			movemExit();
			exit(0);
			break;
		case ' ':
			if ( animation_started == false )
			{
				animation_started = true;
				glutIdleFunc( animate );
			}
			else
			if ( animation_started == true )
			{
				animation_started = false;
				glutIdleFunc( NULL );
			}
			break;
		case 'm':
		case 'M':
			if ( ctrol == VIEWING )
			{
				printf( "switched to track-model mode\n" );
				ctrol = MODELING;
			}
			else
			if ( ctrol == MODELING )
			{
				printf( "switched to track-view mode\n" );
				ctrol = VIEWING;
			}

			glutPostRedisplay();
			break;
		case 'p':
		case 'P':
			if ( proj == PERSPECTIVE )
				proj = ORTHOGRAPHIC;
			else
			if ( proj == ORTHOGRAPHIC )
				proj = PERSPECTIVE;

			glutPostRedisplay();
			break;
		case 'i':
		case 'I':
			if ( fill_mode == GL_LINE )
				fill_mode = GL_FILL;
			else
			if ( fill_mode == GL_FILL )
				fill_mode = GL_POINT;
			else
			if ( fill_mode == GL_POINT )
				fill_mode = GL_LINE;

			glutPostRedisplay();
			break;
		default:
			break;
	}
}

/***************************************************************************\
 *                              Main                                       *
\***************************************************************************/

void commandlineerror( char *cmd, char *parm )
{
	if ( cmd )
		fprintf(stderr, "Offending option: %s\n", cmd );
	if ( parm )
		fprintf(stderr, "with first parameter: %s\n", parm );

	fprintf(stderr, "\n\nUsage: movem options ...\n"
	"Options:\n"
	"<space>     start moving\n"
	"-n num obj  number of moving objects\n"
	"-g obj      geometry type (default = all different kinds)\n"
	"              obj = pl, sh, to\n"
	"-u          coll. det. module runs concurrently\n"
	"-C          level of detection = convex hull only\n"
    "-a          find all colliding polygons\n"
	"-A algo     tell col. which algo should be used\n"
	"			   algo = bx, dp\n"
	"-W #loops   without window; perform #loops many cycles, then exit\n"
    "-G gridsize Use grid with dimension gridsize per axis\n"
	"-h          this help menu\n"
	"Keys:\n"
	"i           switch drawing mode (filled/wireframe/point)\n"
	"p           switch projection mode\n"
	"ESC         quit\n"
	"\n");

	if ( cmd )
		exit(-1);				// problem occured
	else
		exit(0);				// was option -h
}


void parsecommandline( int argc, char *argv[] )
{
	/* valid option characters; last char MUST be 0 ! */
	char optionchar[] =   { 'h', 'g', 'u', 'A', 'C', 'a', 'n', 'W', 'G', '\0' };
	int musthaveparam[] = {  0 ,  1,   0,   1,   0,   0,   1,   1,   1  };
	int mhp[256];
	int isopt[256];
	char optchar;

	unsigned int nopts = strlen(optionchar);
	if ( nopts > 50 )
	{
		fprintf(stderr, "\n\nparsecommandline: the option-chars string "
				"seems to be\nVERY long (%d bytes) !\n\n", nopts );
		exit(-1);
	}

	fill_n( isopt, 256, 0 );
	fill_n( mhp, 256, 0 );
	for ( unsigned int i = 0; i < nopts; i ++ )
	{
		if ( isopt[static_cast<int>(optionchar[i])] )
		{
			fprintf(stderr, "\n\nparsecommandline: Bug: option character '%c'"
					" is specified twice in the\n"
							"option character array !\n\n", optionchar[i] );
			exit(-1);
		}
		isopt[ static_cast<int>(optionchar[i]) ] = 1;
		mhp[ static_cast<int>(optionchar[i])] = musthaveparam[i];
	}

	++argv; --argc;
	while ( argc > 0 )
	{
		if ( argv[0][0] == '-' )
		{
			optchar = argv[0][1];

			if ( ! isopt[static_cast<int>(optchar)] )
			{
				fprintf(stderr, "\nIs not a valid command line option\n");
				commandlineerror( argv[0], NULL );
			}
			for ( int i = 0; i < mhp[static_cast<int>(optchar)]; i ++ )
				if ( ! argv[1+i] || argv[1+i][0] == '-' )
				{
					fprintf(stderr, "\nCommand line option -%c must "
							"have %d parameter(s)\n",
							argv[0][1], mhp[static_cast<int>(optchar)] );
					commandlineerror( argv[0], NULL );
					argv += 1 + i;
					argc -= 1 + i;
					continue;
				}

			switch ( optchar )
			{
				case 'h': commandlineerror( NULL, NULL);  break;
				case 'u': concurrent_coldet = true; break;
				case 'C': level_of_det = col::LEVEL_HULL; break;
				case 'W': with_window = false;
						  NLoops = atoi( argv[1] );
						  break;
				case 'n': nmovingobjs = atoi( argv[1] ); break;
                case 'a': all_polygons = true; break;
				case 'A': if ( ! strcmp(argv[1],"bx") )
							  algo = col::ALGO_BOXTREE;
						  else
						  if ( ! strcmp(argv[1],"dp") )
							  algo = col::ALGO_DOPTREE;
						  break;
				case 'g': if ( ! strcmp(argv[1],"pl") )
							  geom_type = OBJ_PLANES;
						  else
						  if ( ! strcmp(argv[1],"sh") )
							  geom_type = OBJ_SPHERES;
						  else
						  if ( ! strcmp(argv[1],"to") )
							  geom_type = OBJ_TORUS;
						  else
						  {
							  fputs("unrecognized obj type\n",stderr);
							  commandlineerror(argv[0],argv[1]);
						  }
						  break;
				case 'G': with_grid = true;
						  gridsize = atoi( argv[1] );
						  break;

				default: fprintf(stderr, "\nBug in parsecommandline !\n");
						 commandlineerror( argv[0], NULL );
			}

			argv += 1 + mhp[static_cast<int>(optchar)];
			argc -= 1 + mhp[static_cast<int>(optchar)];
		}
		else
		{
			/* command line arg doesn't start with '-' */
			fprintf(stderr, "\nThis is not a valid command line option\n");
			commandlineerror( argv[0], NULL );
			/* or, load file instead .. */
		}
	}

	// check sanity of options
	if ( complexity < 1 ||
		 (geom_type != OBJ_PLANES && complexity < 3) ||
		 complexity > 1000 )
	{
		fprintf(stderr,"complexity (%d) out of range!\n", complexity );
		exit(-1);
	}

	if ( nmovingobjs > MaxNumObjs )
	{
		fprintf(stderr,"number of moving objects (%u) > %d!\n",
				nmovingobjs, MaxNumObjs );
		exit(-1);
	}
}

int main(int argc, char** argv)
{

	// initialize global and static data
    prng = new col::FibRand( time(NULL) );
	DopTree::init();
	viewing_matrix = Matrix4::identity();
	
	// parse command line options
	parsecommandline( argc, argv );

	// initialize scene
	createScene();

	// prepare glut
	glutInit( &argc, argv );
	glutInitDisplayMode ( GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB );
	glutInitWindowSize( 350, 350 ); 
	glutInitWindowPosition( 100, 100 );
	glutCreateWindow( argv[0] );
	init();

	// register glut callback
	glutDisplayFunc( display );
	glutReshapeFunc( reshape );
	glutMouseFunc( mouseClick );
	glutMotionFunc( mouseMoving );
	glutKeyboardFunc( keyboard );

	try
	{
		// this should be done before makeCollidable() or addCallback()!
		pipeline = new col::CollisionPipeline( "CollisionPipelineThread"/*ThreadName*/, 1 /*ThreadID*/);

  		pipeline->setUseHulls( true );
		if( with_grid )
		{
			unsigned int size[3] = { gridsize, gridsize, gridsize };
			REAL min[3] = { -cage_radius, -cage_radius, -cage_radius };
			REAL max[3] = { cage_radius, cage_radius, cage_radius };
			pipeline->useGrid( size, min, max );
		}

		// register objects with collision detection module
		for ( unsigned int i = 0; i < nmovingobjs; i ++ )
			mobj[i].makeCollidable();
		printf("object(geometry)-registration complete\n");
		// register the transformation
		for ( unsigned int i = 0; i < nmovingobjs; i ++ )
			mobj[i].move();
		printf("object(transformation)-registration complete\n");
		// register callbacks
		for ( unsigned int i = 0; i < nmovingobjs; i ++ )
			for ( unsigned int j = 0; j < i; j ++ )
				MovemCallback::addCallback( mobj[i], mobj[j], level_of_det );
		printf("object(callback)-registration complete\n");

		if ( concurrent_coldet )
		{
			CollisionSyncFun *syncfun = new CollisionSyncFun();
			pipeline->setSyncFun( syncfun );

			pt = pipeline->runConcurrently( NULL );
			if ( pt!=NULL )
				printf("pipeline thread starts\n");
		}
	}
	catch ( col::XCollision &x )
	{
		fputs("movem: collision exception!\n",stderr);
		x.print();
		exit(-1);				// in a real app, we would try to continue
	}

	// run...
	if ( with_window )
    {
		glutMainLoop();
    }
	else
	{
		for ( unsigned int i = 0; i < NLoops; i ++ )
			move_and_check();
		movemExit();
	}

	return 0;
}

